#ifndef ANIMATION_MEMU_H
#define ANIMATION_MEMU_H

#ifndef OLED_FPS
#define OLED_FPS 60
#endif

#include <U8g2lib.h>
#include "PinMap.h"
#include "StepMotor.h"
#include "FashionStar_UartServo.h" 

#define MASS 0.6
#define DAMPING 4.5
#define SPRING_CONSTANT 25
#define BOOST 3
#define MAX_BOUNCE 3 
#define PAGE_MAX_LINES 7
#define ENDSTR "\\END"

bool flag_u8g2_show_msg=false;

char* connecting_page[]{
    "WIFI Connecting ...",
    "SSID: ",
    NULL,
    NULL,
    ENDSTR
};

char* test_page[]{
    "TEST1",
    "TEST1",
    "TEST1",
    ENDSTR
};

char* blank_page[4]{
    NULL,
    ENDSTR
};

// 弹簧阻尼模型的递推实现
float step_f(float time_step, float mass, float c, float k, float * fvx){
    return k * fvx[2] - c * fvx[1];
} 
float step_v(float time_step, float mass, float c, float k, float * fvx){
    return fvx[1] + ((fvx[0] / mass) * time_step);
} 
float step_x(float time_step, float mass, float c, float k, float * fvx){
    return fvx[2] - (fvx[1] * time_step);
} 

// 渲染单页内容，不刷新屏幕
void render_page(U8G2* u8g2, char* page[], int x, int y){
    for(int i = 0; i < PAGE_MAX_LINES; i++){
        if(page[i] == NULL){continue;}
        if(page[i] == ENDSTR){break;}
        if (i == 0){ u8g2->drawStr(x, y+10, page[i]);}  // 第一行间隔稍微宽一点
        else if (i == 1){ u8g2->drawStr(x, y+(14*i)+10, page[i]);}  // 第一行间隔稍微宽一点
        else {u8g2->drawStr(x, y+(10*i)+14, page[i]);}
    }
}

// 自动换行打印，包含了换行符以及过长的消息
void u8g2print_auto_newline(U8G2* u8g2, char* income_str, uint8_t maxlen, uint8_t y, uint8_t x){
    char tempLine[31];
    uint8_t line_now = 0;
    if(maxlen > 20){maxlen = 20;}
    for(int i=0;i<255;i++){
        if(income_str[i]!='\n' && income_str[i]!='\r' && income_str[i]!='\0' && line_now < maxlen){
            tempLine[line_now] = income_str[i];
            line_now ++;
        }else{
            if(income_str[i]=='\n' || income_str[i]=='\r'){
                tempLine[line_now + 1] = '\0';
                u8g2->drawStr(x, y, tempLine);
                y = y + 10;
                line_now = 0;
            }else if (income_str[i]=='\0'){
                tempLine[line_now] = '\0';
                u8g2->drawStr(x, y, tempLine);
                break;
            }else if (line_now >= maxlen){
                tempLine[line_now+1] = '\0';
                u8g2->drawStr(x, y, tempLine);
                y = y + 10;
                line_now = 0;
                tempLine[line_now] = income_str[i];
                line_now ++;
            }
        }
    }
}

// 渲染两个页面切换的弹簧效果，阻塞式
void change_page(U8G2* u8g2, char* page1[], char* page2[], uint16_t max_time=500){
    int frames = max_time * OLED_FPS / 1000;
    int delay_ms = 1000 / OLED_FPS;
    uint8_t bounce = 0;
    float time_step = 1. / OLED_FPS;
    float _fvx[3] = {0., 0., 128.};
    float last_x = 0.;
    for(int i = 0; i < frames && bounce < MAX_BOUNCE; i++){
        // 更新模型
        for(int i = 0; i < BOOST; i++){
            _fvx[0] = step_f(time_step, MASS, DAMPING, SPRING_CONSTANT, _fvx);
            _fvx[1] = step_v(time_step, MASS, DAMPING, SPRING_CONSTANT, _fvx);
            _fvx[2] = step_x(time_step, MASS, DAMPING, SPRING_CONSTANT, _fvx);
        }
        // 统计回弹次数
        if(last_x * _fvx[2] < 0) {bounce ++ ;}
        last_x = _fvx[2];
        u8g2->clearBuffer();
        render_page(u8g2, page1, 128-_fvx[2], 0);
        render_page(u8g2, page2, -_fvx[2], 0);
        u8g2->sendBuffer();
        delay(delay_ms);
        // Serial.printf("%d\t%f\t%f\t%F\n", i, _fvx[0], _fvx[1], _fvx[2]);
    }
    u8g2->clearBuffer();
    render_page(u8g2, page2, 0, 0);
    u8g2->sendBuffer();
}

char* init_msg_page[]{
    NULL,
    NULL,
    NULL,
    NULL,
    NULL,
    "\\END"
};

char* hw_msg_page[7]{
    NULL,
    NULL,
    NULL,
    NULL,
    NULL,
    NULL,
    "\\END"
};

void gen_servo_stat_str(uint8_t num, FSUS_Servo * uservo, char * char_buffer){
    if (uservo->isOnline) {
        sprintf(char_buffer, " Servo %d: %s", num, "Online");
    }else{
        sprintf(char_buffer, " Servo %d: %s", num, "Offline");
    }
}

void robo_init_display(U8G2_SSD1306_128X64_NONAME_F_HW_I2C * u8g2, bool servo1_stat, bool servo2_stat)
{
    // u8g2->setFont(u8g2_font_ncenB08_tr);
    // char servo1_stat_char[20];
    char* servo1_stat_char = new char[20];
    // char servo2_stat_char[20];
    char* servo2_stat_char = new char[20];
    if (BOARD_TYPE == 0){
        // u8g2->drawStr(0, 10, "Board Type: PICO-D4");
        init_msg_page[0] = new char[sizeof("Board Type: PICO-D4")];
        init_msg_page[0] = "Board Type: PICO-D4";
        // init_msg_page[0] = new char[sizeof("Board Type: PICO-D4")] {"Board Type: PICO-D4"};
    }else if(BOARD_TYPE == 1){
        // u8g2->drawStr(0, 10, "Board Type: ESP32-C3");
        init_msg_page[0] = new char[sizeof("Board Type: ESP32-C3")];
        init_msg_page[0] = "Board Type: ESP32-C3";
        // init_msg_page[0] = new char[sizeof("Board Type: ESP32-C3")] {"Board Type: ESP32-C3"};
    }
    init_msg_page[1] = "ServoState:";
    // u8g2->drawStr(0, 20, "ServoState:");
    // u8g2->drawStr(10, 30, "Servo 1:");
    if (servo1_stat) {
        sprintf(servo1_stat_char, " Servo 1: %s", "Online");
    }else{
        sprintf(servo1_stat_char, " Servo 1: %s", "Offline");
    }
    // sprintf(servo_stat_char, "%d", servo1_stat);
    init_msg_page[2] = servo1_stat_char;
    // u8g2->drawStr(80, 30, servo_stat_char);
    // u8g2->drawStr(10, 40, "Servo 2:");
    if (servo2_stat) {
        sprintf(servo2_stat_char, " Servo 2: %s", "Online");
    }else{
        sprintf(servo2_stat_char, " Servo 2: %s", "Offline");
    }
    // sprintf(servo_stat_char, "%d", servo2_stat);
    init_msg_page[3] = servo2_stat_char;
    init_msg_page[4] = connecting_page[3];

    change_page(u8g2, connecting_page, init_msg_page);
    
    vTaskDelay(1000);
    char c_hw_uart1[30];
    char c_hw_uart2[30] ;
    char c_hw_motor1[30];
    char c_hw_motor2[30];
    char c_hw_i2c[30];

    sprintf(c_hw_uart1,  "TX1: %d, RX1: %d", TX_1, RX_1);
    sprintf(c_hw_uart2,  "TX2: %d, RX2: %d", TX_2, RX_2);
    sprintf(c_hw_motor1, "PUL1: %d, DIR1: %d", PUL_1, DIR_1);
    sprintf(c_hw_motor2, "PUL2: %d, DIR2: %d", PUL_2, DIR_2);
    sprintf(c_hw_i2c,  "SDA : %d, SCL : %d", I2C_SDA, I2C_SCL);
    // u8g2->drawStr(0, 10, "HardWare Definition:");
    // u8g2->drawStr(10,  20, c_hw_uart1);
    // u8g2->drawStr(10,  30, c_hw_uart2);
    // u8g2->drawStr(10,  40, c_hw_motor1);
    // u8g2->drawStr(10,  50, c_hw_motor2);
    // u8g2->drawStr(10,  60, c_hw_i2c);
    for (int i = 0; i < 6; i++) {
        hw_msg_page[i] = (char*)malloc(30 * sizeof(char));
    }

    strcpy(hw_msg_page[0], "HardWare Definition");
    strcpy(hw_msg_page[1], c_hw_uart1);
    strcpy(hw_msg_page[2], c_hw_uart2);
    strcpy(hw_msg_page[3], c_hw_motor1);
    strcpy(hw_msg_page[4], c_hw_motor2);
    strcpy(hw_msg_page[5], c_hw_i2c);


    // hw_msg_page[0] = "HardWare Definition";
    // hw_msg_page[1] = c_hw_uart1;
    // hw_msg_page[2] = c_hw_uart2;
    // hw_msg_page[3] = c_hw_motor1;
    // hw_msg_page[4] = c_hw_motor2;
    // hw_msg_page[5] = c_hw_i2c;
    // u8g2->drawStr(0,   90, c_hw_uservo);
    // u8g2->sendBuffer();
    // serial.printf("\nI2C_SDA :\t%d, \tI2C_SCL :\t%d\n", I2C_SDA, I2C_SCL);
    change_page(u8g2, init_msg_page, hw_msg_page);
}

char* state_page[7]{
    "Umeko Lab",
    "  0.00      0.00",
    "  0.00      0.00",
    "power:",
    "  0.00 W    0.00 W",
    "  0.00 W    0.00 W",
    "\\END"
};

#if defined(HAVE_INA226)
void motor_stat_display(U8G2_SSD1306_128X64_NONAME_F_HW_I2C * u8g2, 
                        StepMotor * motor_0, StepMotor * motor_1, 
                        FSUS_Servo * uservo0, FSUS_Servo * uservo1,
                        PowerManager * PM1, PowerManager * PM2, char* msg)
#else
void motor_stat_display(U8G2_SSD1306_128X64_NONAME_F_HW_I2C * u8g2, 
                        StepMotor * motor_0, StepMotor * motor_1, 
                        FSUS_Servo * uservo0, FSUS_Servo * uservo1, char* msg)
#endif
{
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = 1000/OLED_FPS;
    u8g2->setFont(u8g2_font_ncenB08_tr);
    char output_motor1[10];
    char output_motor2[10];
    char output_servo1[10];
    char output_servo2[10];
    char lim_pin[20];
    // char lim_pin2[15];
    char stat_1[4];
    char stat_2[4];
    // #if defined(HAVE_INA226)
    char power_char[20];
    // #endif
    change_page(u8g2, hw_msg_page, state_page);
    while(true){
        u8g2->clearBuffer();
        // -----电机速度------
        sprintf(output_motor1, "%.2f", motor_0->freqSpeed);
        sprintf(output_motor2, "%.2f", motor_1->freqSpeed);
        sprintf(output_servo1, "%.2f", uservo0->speed);
        sprintf(output_servo2, "%.2f", uservo1->speed);
        // #if defined(USE_WIFI)
        u8g2->drawStr(0, 10, msg);
        // #else
        // u8g2->drawStr(0, 10, "motor spd:");
        // #endif
        u8g2->drawStr(10, 20, output_motor1);
        u8g2->drawStr(70, 20, output_motor2);
        u8g2->drawStr(10, 30, output_servo1);
        u8g2->drawStr(70, 30, output_servo2);
        // -----限位状态------
        // if (motor_0->positivePinValue == LIM_SENSOR_PIN_1_TRIG_LEVEL){
        //     sprintf(stat_1, "%s", "On");
        // }else{
        //     sprintf(stat_1, "%s", "Off");
        // }
        // if (motor_0->negativePinValue == LIM_SENSOR_PIN_1_TRIG_LEVEL){
        //     sprintf(stat_2, "%s", "On");
        // }else{
        //     sprintf(stat_2, "%s", "Off");
        // }
        // sprintf(lim_pin, "P %d: %s, P %d: %s", LIM_SENSOR_PIN_1, stat_1, LIM_SENSOR_PIN_2, stat_2);
        // u8g2->drawStr(0, 40, "lim_sensor:");
        // u8g2->drawStr(10, 50, lim_pin);
        // ----------------电源信息-------------------
        u8g2->drawStr(0, 40, "Power Info:");
        #if defined(HAVE_INA226)
        sprintf(power_char, "%.2f W   %.2f W", PM1->getPower(), PM2->getPower());
        // printf("%.2f W\t", PM1.getPower());
        u8g2->drawStr(10, 50, power_char);
        #endif
        sprintf(power_char, "%.2f W   %.2f W", uservo0->power, uservo1->power);
        u8g2->drawStr(10, 60, power_char);
        u8g2->sendBuffer();
        
        if(flag_u8g2_show_msg){
            gen_servo_stat_str(0, uservo0, init_msg_page[2]);
            gen_servo_stat_str(1, uservo1, init_msg_page[3]);
            change_page(u8g2, state_page, init_msg_page);
            vTaskDelay(1000);
            change_page(u8g2, init_msg_page, state_page);
            flag_u8g2_show_msg = false;
        }
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
    }
}

#endif